#pragma once
#include <cmath>
#include <memory>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <mutex>
#include <map>

#include "common_lib/common.h"
#include "common_lib/log.h"

#include "control/move_control/move_control.h"
#include "control_params.h"
#include "custom_class.h"
#include "control_params.h"

namespace control
{
    class MoveControl;

    class Control
    {
    public: 
        Control(ros::NodeHandle &nh, ros::NodeHandle &private_nh);
        ~Control();
        void Start();
    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
        std::shared_ptr<MoveControl> ptr_move_control_;

        std::shared_ptr<AMRState> ptr_amr_state_;
        std::shared_ptr<control_msgs::control_msg> ptr_msg_control_;
        std::shared_ptr<control_msgs::control_state_msg> ptr_msg_control_state_;

        ros::Publisher pub_control_;
        ros::Publisher pub_control_state_;
        ros::Subscriber sub_trajectory_;
        ros::Subscriber sub_actuator_;
        ros::Subscriber sub_localization_;

        std::multimap<uint32_t, FsmState> all_task_info_; //所有模块的任务执行情况
        std::multimap<uint32_t, ros::Time> all_task_info_time_;
    
		uint32_t cur_task_id_ = 0; //当前在执行的task id
		std::string cur_fsm_state_ = "";//当前任务的状态
		std::string amr_state = "";//当前车体的状态
		bool is_simulation_ = false;//是否为仿真模式

        std::string map_file_;
        std::string vehicle_param_file_;
        std::string file_path_;

        void Init();
        void InitLog();
        void LoadParams();

        void ActuatorCallback(const actuator_msgs::actuator_msgConstPtr &msg);
        void TrajectoryCallback(const amr_trajectory_msgs::amr_trajectory_msgConstPtr &msg);
        void LocalizationCallback(const localization_msgs::localization_msgConstPtr &msg);

		void PostProcess();		//后处理函数
		void ResetTaskCommond();//重置任务指令
		void PublishMessage();
		//记录csv log
		void TraceLog();

		void UpdateTotalFsm();//大状态机更新
		void SummaryFsmStateForDispatch();//根据所有状态机信息汇总结果
		void UpdateAMRState();//更新车端状态
    };
}